#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

using namespace std;
using namespace cv;
int j=1;
int main(int argc, char** argv)
{
    ros::init(argc, argv, "video_capture");
    ros::NodeHandle nh;                           // 声明节点句柄
//    ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("image1", 1000);
    image_transport::ImageTransport it(nh);
    // 告诉节点管理器（roscore）我们要在camera1/image1话题上发布图像，
    // 这里第一个参数是话题的名称，第二个是缓冲区的大小（即消息队列的长度，在发布图像消息时消息队列的长度只能是1）
    image_transport::Publisher pub = it.advertise("/camera/image_raw", 1);
    image_transport::Publisher pub1 = it.advertise("mask", 1);
    
   
    VideoCapture vcap1;
    Mat image1;
/*******************打开图像**********************/
    // Mat msg_mask=imread("/home/chris/demo_video_processing/ganpicimg1.jpg",CV_LOAD_IMAGE_COLOR);//李霖杰梯度下降测试用图
    Mat msg_mask=imread("/home/chris/demo_video_processing/Trim3_mask.png",CV_LOAD_IMAGE_COLOR);//RM官方比赛视频测试用掩模
    
/********************打开视频**********************/
    const string videoStreamAddress1 = "/home/chris/RM_video/video/标准版.mp4";//左清宇C4D渲染视频(雷达站视角)
    // const string videoStreamAddress1 = "/home/chris/RM_video/djtu_vs_sjtu_Trim3.mp4";//RM比赛视频
          if(!vcap1.open(videoStreamAddress1))
      {
          cout << "Error opening video stream or file" << endl;
          return -1;
      }

    sensor_msgs::ImagePtr msg;
    sensor_msgs::ImagePtr msg1;
    ros::Rate loop_rate(30);
    while (ros::ok())  // 不能写while(1),否则程序无法用ctrl+c关掉
    {
        vcap1.read(image1);

        // OpenCV格式的图像转化为ROS中的消息
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image1).toImageMsg(); // 根据运行时给定的参数（图像文件的路径）读取图像
        msg1=cv_bridge::CvImage(std_msgs::Header(),"bgr8",msg_mask).toImageMsg();
        pub.publish(msg);
        pub1.publish(msg1);
        loop_rate.sleep();
       // waitKey(1);
    }
    return 0;
}
